#include "drone_serial_interface/drone_serial_interface_node.hpp"
#include <string>
#include <mutex>
#include <chrono>
#include <memory>
#include <functional>
#include <vector>
#include <iomanip>
#include <sstream>
#include <rclcpp/rclcpp.hpp>
#include <geometry_msgs/msg/twist.hpp>
#include <std_msgs/msg/string.hpp>
#include <diagnostic_msgs/msg/diagnostic_status.hpp>
#include <diagnostic_updater/diagnostic_updater.hpp>
#include <rcl_interfaces/msg/set_parameters_result.hpp>
#include <serial/serial.h>

namespace drone_serial_interface {

// ========== Safety and Monitoring Functions ==========

void DroneSerialInterfaceNode::emergencyStop()
{
  geometry_msgs::msg::Twist stop_cmd;
  // All velocities are zero by default
  
  if (serial_connected_) {
    sendVelocityCommand(stop_cmd);
    RCLCPP_WARN(get_logger(), "Emergency stop command sent");
  }
  
  // Update last command
  {
    std::lock_guard<std::mutex> lock(cmd_vel_mutex_);
    last_cmd_vel_ = stop_cmd;
    last_cmd_time_ = std::chrono::steady_clock::now();
  }
}

bool DroneSerialInterfaceNode::isCommandTimeout()
{
  std::lock_guard<std::mutex> lock(cmd_vel_mutex_);
  auto now = std::chrono::steady_clock::now();
  auto duration = std::chrono::duration_cast<std::chrono::milliseconds>(
    now - last_cmd_time_).count();
  
  return duration > (cmd_timeout_ * 1000.0);
}

void DroneSerialInterfaceNode::logSerialError(const std::string & error_msg)
{
  RCLCPP_ERROR(get_logger(), "Serial Error: %s", error_msg.c_str());
  
  // Publish error status
  std_msgs::msg::String status_msg;
  status_msg.data = "error: " + error_msg;
  if (serial_status_pub_) {
    serial_status_pub_->publish(status_msg);
  }
}

// ========== Diagnostic Callbacks ==========

void DroneSerialInterfaceNode::diagnosticConnectionStatus(
  diagnostic_updater::DiagnosticStatusWrapper & stat)
{
  if (serial_connected_ && serial_port_ && serial_port_->isOpen()) {
    stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "Serial connection active");
    stat.add("Port", serial_device_);
    stat.add("Baudrate", std::to_string(serial_baudrate_));
    stat.add("Status", "Connected");
  } else {
    stat.summary(diagnostic_msgs::msg::DiagnosticStatus::ERROR, "Serial connection failed");
    stat.add("Port", serial_device_);
    stat.add("Baudrate", std::to_string(serial_baudrate_));
    stat.add("Status", "Disconnected");
  }
}

void DroneSerialInterfaceNode::diagnosticVelocityStatus(
  diagnostic_updater::DiagnosticStatusWrapper & stat)
{
  std::lock_guard<std::mutex> lock(cmd_vel_mutex_);
  
  if (isCommandTimeout()) {
    stat.summary(diagnostic_msgs::msg::DiagnosticStatus::WARN, "Command timeout");
    stat.add("Timeout (s)", std::to_string(cmd_timeout_));
  } else {
    stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "Velocity commands active");
  }
  
  stat.add("Last VX", std::to_string(last_cmd_vel_.linear.x));
  stat.add("Last VY", std::to_string(last_cmd_vel_.linear.y));
  stat.add("Last VZ", std::to_string(last_cmd_vel_.linear.z));
  stat.add("Last WZ", std::to_string(last_cmd_vel_.angular.z));
  
  auto now = std::chrono::steady_clock::now();
  auto duration = std::chrono::duration_cast<std::chrono::milliseconds>(
    now - last_cmd_time_).count();
  stat.add("Time since last command (ms)", std::to_string(duration));
}

void DroneSerialInterfaceNode::diagnosticProtocolStatus(
  diagnostic_updater::DiagnosticStatusWrapper & stat)
{
  stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "Protocol operational");
  stat.add("Protocol Type", protocol_type_);
  stat.add("Max Linear Velocity", std::to_string(max_linear_velocity_));
  stat.add("Max Angular Velocity", std::to_string(max_angular_velocity_));
  stat.add("Safety Checks", enable_safety_checks_ ? "Enabled" : "Disabled");
}

// ========== Parameter Management ==========

void DroneSerialInterfaceNode::declareParameters()
{
  // Serial communication parameters
  declare_parameter<std::string>("serial_device", "/dev/ttyUSB0");
  declare_parameter<int>("serial_baudrate", 115200);
  declare_parameter<int>("serial_timeout_ms", 1000);
  
  // Protocol parameters
  declare_parameter<std::string>("protocol_type", "custom");
  declare_parameter<double>("cmd_timeout", 1.0);
  
  // Safety parameters
  declare_parameter<double>("max_linear_velocity", 5.0);
  declare_parameter<double>("max_angular_velocity", 2.0);
  declare_parameter<bool>("enable_safety_checks", true);
  
  // Set parameter callback
  auto param_callback = std::bind(&DroneSerialInterfaceNode::parametersCallback, 
    this, std::placeholders::_1);
  param_callback_handle_ = add_on_set_parameters_callback(param_callback);
}

void DroneSerialInterfaceNode::getParameters()
{
  // Get serial parameters
  serial_device_ = get_parameter("serial_device").as_string();
  serial_baudrate_ = get_parameter("serial_baudrate").as_int();
  int timeout_ms = get_parameter("serial_timeout_ms").as_int();
  
  // Set serial timeout
  serial_timeout_ = serial::Timeout::simpleTimeout(timeout_ms);
  
  // Get protocol parameters
  protocol_type_ = get_parameter("protocol_type").as_string();
  cmd_timeout_ = get_parameter("cmd_timeout").as_double();
  
  // Get safety parameters
  max_linear_velocity_ = get_parameter("max_linear_velocity").as_double();
  max_angular_velocity_ = get_parameter("max_angular_velocity").as_double();
  enable_safety_checks_ = get_parameter("enable_safety_checks").as_bool();
  
  RCLCPP_INFO(get_logger(), 
    "Parameters loaded: device=%s, baudrate=%d, protocol=%s",
    serial_device_.c_str(), serial_baudrate_, protocol_type_.c_str());
}

rcl_interfaces::msg::SetParametersResult DroneSerialInterfaceNode::parametersCallback(
  const std::vector<rclcpp::Parameter> & parameters)
{
  rcl_interfaces::msg::SetParametersResult result;
  result.successful = true;
  
  for (const auto & param : parameters) {
    if (param.get_name() == "max_linear_velocity") {
      max_linear_velocity_ = param.as_double();
      RCLCPP_INFO(get_logger(), "Updated max_linear_velocity: %.3f", max_linear_velocity_);
    } else if (param.get_name() == "max_angular_velocity") {
      max_angular_velocity_ = param.as_double();
      RCLCPP_INFO(get_logger(), "Updated max_angular_velocity: %.3f", max_angular_velocity_);
    } else if (param.get_name() == "enable_safety_checks") {
      enable_safety_checks_ = param.as_bool();
      RCLCPP_INFO(get_logger(), "Updated enable_safety_checks: %s", 
        enable_safety_checks_ ? "true" : "false");
    } else if (param.get_name() == "cmd_timeout") {
      cmd_timeout_ = param.as_double();
      RCLCPP_INFO(get_logger(), "Updated cmd_timeout: %.3f", cmd_timeout_);
    } else if (param.get_name() == "protocol_type") {
      protocol_type_ = param.as_string();
      RCLCPP_INFO(get_logger(), "Updated protocol_type: %s", protocol_type_.c_str());
    }
  }
  
  return result;
}

// ========== Utility Functions ==========

std::string DroneSerialInterfaceNode::getTimestamp()
{
  auto now = std::chrono::system_clock::now();
  auto time_t = std::chrono::system_clock::to_time_t(now);
  auto ms = std::chrono::duration_cast<std::chrono::milliseconds>(
    now.time_since_epoch()) % 1000;
  
  std::ostringstream oss;
  oss << std::put_time(std::localtime(&time_t), "%Y-%m-%d %H:%M:%S");
  oss << '.' << std::setfill('0') << std::setw(3) << ms.count();
  
  return oss.str();
}

} // namespace drone_serial_interface

// Component registration
#include "rclcpp_components/register_node_macro.hpp"
RCLCPP_COMPONENTS_REGISTER_NODE(drone_serial_interface::DroneSerialInterfaceNode)